import rospy
from sensor_msgs.msg import JointState

class MasterArmSim:
    def __init__(self):
        rospy.init_node('master_arm_sim', anonymous=True)
        # 发布主臂关节状态到 /master/joint_left 话题
        self.pub = rospy.Publisher('/master/joint_left', JointState, queue_size=10)

    def publish_joint_states(self):
        # 创建并发布主臂的 JointState 消息
        msg = JointState()
        msg.header.stamp = rospy.Time.now()
        # 主臂关节名称
        msg.name = ["master_joint1", "master_joint2", "master_joint3", "master_joint4", "master_joint5", "master_joint6"]
        # 主臂关节位置数据（模拟数据）
        msg.position = [6, 5, 4, 3, 2, 1]  # 模拟的主臂位置数据
        self.pub.publish(msg)
        # rospy.loginfo("Published master arm position: %s", msg.position)

    def run(self):
        rate = rospy.Rate(30)  # 30 Hz
        while not rospy.is_shutdown():
            self.publish_joint_states()
            rate.sleep()

if __name__ == '__main__':
    controller = MasterArmSim()
    try:
        controller.run()
    except rospy.ROSInterruptException:
        pass